from my_action.srv import JointPosition

import rclpy
import math

from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
import tf_transformations

class JointPositionServer(Node):

    def __init__(self):
        super().__init__('joint_position_server')
        self.srv = self.create_service(JointPosition, 'joint_position', self.joint_position_callback)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

    def transform_joint_distance(self, joint_type):
        now = rclpy.time.Time()
        if joint_type == "raise":
            child_frame = "arm_throttle"
            parent_frame = "cab"
            trans = self.tf_buffer.lookup_transform(
                child_frame,
                parent_frame,
                now)
            euler = tf_transformations.euler_from_quaternion([
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w     
            ])
            distance = euler[1]/math.pi*180

        elif joint_type == "extend":
            child_frame = "tof"
            parent_frame = "arm_end"
            trans = self.tf_buffer.lookup_transform(
                child_frame,
                parent_frame,
                now)
            distance = trans.transform.translation.x

        elif joint_type == "rotate":
            child_frame = "cab"
            parent_frame = "car"
            trans = self.tf_buffer.lookup_transform(
                child_frame,
                parent_frame,
                now)
            euler = tf_transformations.euler_from_quaternion([
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w     
            ])
            distance = euler[2]/math.pi*180

        elif joint_type == "lift":
            child_frame = "hook"
            parent_frame = "arm_end"
            trans = self.tf_buffer.lookup_transform(
                child_frame,
                parent_frame,
                now)
            distance = trans.transform.translation.z

        return distance

    def joint_position_callback(self, request, response):
        response.joint_position = self.transform_joint_distance(request.joint_type)
        print("Get")
        self.get_logger().info('Joint type: ' + request.joint_type + ' Position: ' + str(response.joint_position))
        return response


def main(args=None):
    rclpy.init(args=args)

    minimal_service = JointPositionServer()

    rclpy.spin(minimal_service)

    rclpy.shutdown()


if __name__ == '__main__':
    main()